#include "main.h"
#include "./SYSTEM/delay/delay.h"
#include "math.h"
#include "Lowpass_Filter.h"

/* 定义2π常量 */
#define _2PI       6.28318530718f

/* 简单低通滤波器的上一次输出值 */
float y = 0;

/**
 * @brief   简单一阶低通滤波器
 * @param   x - 输入信号值
 * @retval  滤波后的输出信号值
 * @note    采用固定系数的一阶低通滤波算法，公式：y(n) = 0.9*x(n) + 0.1*y(n-1)
 */
float Lowpassfilter_sim(float x)
{
	float out = 0.9*x + 0.1*y;  /* 当前输出 = 0.9*当前输入 + 0.1*上一次输出 */
	y = x;  /* 保存当前输入作为下一次的历史输入 */
	return out;  /* 返回滤波结果 */
}

/**
 * @brief   时变一阶低通滤波器
 * @param   lowpass - 低通滤波器结构体指针，包含滤波参数和历史数据
 * @param   x - 输入信号值
 * @retval  滤波后的输出信号值
 * @note    根据时间间隔动态调整滤波系数的一阶低通滤波算法
 *          公式：y(n) = α*y(n-1) + (1-α)*x(n)，其中α = Tf/(Tf+dt)
 */
float Lowpassfilter(struct LOWPASS *lowpass, float x)
{
	float dt = 0.0;  /* 时间间隔（单位：秒） */

	/* 计算当前时间戳与上一次时间戳的差值 */
	uint32_t Timesamp = SysTick->VAL;  /* 获取当前系统滴答计数器值 */
	if(Timesamp < lowpass->Last_Timesamp)  /* 如果当前计数值小于上一次计数值（发生了溢出） */
		dt = (float)(lowpass->Last_Timesamp - Timesamp)/9*1e-6;  /* 计算时间差（系统时钟频率为9MHz） */
	else
		dt = (float)(0xFFFFFF - Timesamp + lowpass->Last_Timesamp)/9*1e-6;  /* 处理计数器溢出情况 */

	/* 时间间隔合理性检查与修正 */
	if(dt<0.0||dt==0)  /* 如果时间差为负或零 */
		dt = 0.0015f;  /* 设置默认时间差为1.5ms */
	else if(dt>0.005f)  /* 如果时间差过大（超过5ms） */
	{
		lowpass->Last_y = x;  /* 直接更新历史输出为当前输入 */
		lowpass->Last_Timesamp = Timesamp;  /* 更新时间戳 */
		return x;  /* 直接返回当前输入值 */
	}

	/* 计算滤波系数并执行滤波 */
	float alpha = lowpass->Tf / (lowpass->Tf + dt);  /* 计算滤波系数α，Tf为滤波时间常数 */
	float y = alpha * lowpass->Last_y + (1.0f - alpha) * x;  /* 执行一阶低通滤波计算 */
	
	/* 更新滤波器状态 */
	lowpass->Last_y = y;  /* 保存当前输出作为下一次的历史输出 */
	lowpass->Last_Timesamp = Timesamp;  /* 更新时间戳 */

	return y;  /* 返回滤波结果 */
}
